#include "RNDF.h"
#include <math.h>
#include <sys/time.h>
#include <unistd.h>


#ifndef M_PI
#define M_PI           3.14159265358979323846
#endif

//#include "ListHandle.h"

double GPSdistance(double lat1, double long1, double lat2, double long2);

double GPSdistance(double lat1, double long1, double lat2, double long2)
    {
    //return distance(meters) between two GPS points
    double dist_m;
    double phi1,theta1,phi2,theta2;
    double x1,y1,z1,x2,y2,z2;

    phi1 = lat1>0? 90-lat1:90+lat1;
    theta1 = long1>0?long1:long1*(-1);

    phi1 = phi1*M_PI/180.0;
    theta1 = theta1*M_PI/180.0;

    phi2 = lat2>0? 90-lat2:90+lat2;
    theta2 = long2>0?long2:long2*(-1);

    phi2 = phi2*M_PI/180.0;
    theta2 = theta2*M_PI/180.0;

    x1 = 6367000*cos(theta1)*sin(phi1);
    y1 = 6367000*sin(theta1)*sin(phi1);
    z1 = 6367000*cos(phi1);

    x2 = 6367000*cos(theta2)*sin(phi2);
    y2 = 6367000*sin(theta2)*sin(phi2);
    z2 = 6367000*cos(phi2);

    //std::cout << "Phi1="<<phi1<<"theta1"<<theta1<<std::endl;
    //std::cout << "Phi2="<<phi1<<"theta2"<<theta2<<std::endl;

    dist_m = sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2)+(z1-z2)*(z1-z2));

    return dist_m;

    }


int main()
    {
    struct timeval start, end;

    long mtime, seconds, useconds;        

    string rndf_file_name="MountainSpeedway.rndf";
    
    gettimeofday(&start, NULL);
    RNDF rndf=RNDF(rndf_file_name,true);    
     gettimeofday(&end, NULL);
     
     seconds  = end.tv_sec  - start.tv_sec;
    useconds = end.tv_usec - start.tv_usec;

    mtime = ((seconds) * 1000 + useconds/1000.0) + 0.5;

    printf("Elapsed time: %ld milliseconds\n", mtime);
     
    ElementID e1=ElementID(1,1,1);
    cout<<"----My Own---: "<<rndf.phaseNo_map[e1]<<endl;

    }